/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-05-13     Administrator       the first version
 */
#include "Gyroscope.h"


rt_device_t serial;            // 设备句柄
char rx_buffer[RX_BUFFER_SIZE];// 接收缓冲区

// 定义接收变量
uint8_t RollL, RollH, PitchL, PitchH, YawL, YawH, VL, VH, SUM;
float Pitch,Roll,Yaw;
uint8_t receivedData[9];

//环形缓冲区
struct rt_ringbuffer gyro_rx_rb;
uint8_t gyro_rx_raw_buf[RX_RINGBUF_SIZE];  // 后台接收缓冲

/*硬件接口初始化*/
int uart2_dma_init(void)
{
    /* 查找串口设备 */
    serial = rt_device_find(SAMPLE_UART_NAME);
    if (!serial) {
        rt_kprintf("UART device %s not found!\n", SAMPLE_UART_NAME);
        return RT_ERROR;
    }

    /* 配置串口参数 */
    struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
    config.baud_rate = BAUD_RATE_9600;
    config.bufsz = 256;
    rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config);

    /* 以DMA接收+轮询发送模式打开设备 */
    if (rt_device_open(serial, RT_DEVICE_FLAG_DMA_RX) != RT_EOK) {
        rt_kprintf("Failed to open UART!\n");
        return RT_ERROR;
    }

    // 初始化 ringbuffer
    rt_ringbuffer_init(&gyro_rx_rb, gyro_rx_raw_buf, RX_RINGBUF_SIZE);


    /* 设置接收回调 */
    rt_device_set_rx_indicate(serial, uart2_dma_callback);

    rt_kprintf("Gyroscope enable succeed\r\n");

    return RT_EOK;
}


/*DMA中断回调接收函数*/
rt_err_t uart2_dma_callback(rt_device_t dev, rt_size_t size)
{
    rt_size_t read_len = rt_device_read(dev, 0, rx_buffer, RX_BUFFER_SIZE);
    if (read_len > 0)
    {
        rt_ringbuffer_put(&gyro_rx_rb, (uint8_t *)rx_buffer, read_len);
    }
    return RT_EOK;
}

/* 阻塞发送单字节 */
void jy61p_send_byte(uint8_t data)
{
    rt_size_t sent = rt_device_write(serial, 0, &data, 1);
    if (sent != 1) {
        rt_kprintf("[WARN] Send failed: 0x%02X\n", data);
    }
}

void Gyroscope_task(void *parameter)
{
    rt_tick_t last_wake_time = rt_tick_get();

    static uint8_t data_index = 0;
    static uint8_t rx_state = WAIT_HEADER1;

    while (1)
    {
        rt_thread_delay_until(&last_wake_time, 10);
//        rt_kprintf("[GyroTask] Running at tick %u\n", rt_tick_get());

        uint8_t temp;
        while (rt_ringbuffer_getchar(&gyro_rx_rb, &temp) == 1)
        {
            switch (rx_state)
            {
            case WAIT_HEADER1:
                if (temp == 0x55)
                    rx_state = WAIT_HEADER2;
                break;
            case WAIT_HEADER2:
                rx_state = (temp == 0x53) ? RECEIVE_DATA : WAIT_HEADER1;
                data_index = 0;
                break;
            case RECEIVE_DATA:
                receivedData[data_index++] = temp;
                if (data_index >= 9)
                {
                    process_euler_angles();
                    rx_state = WAIT_HEADER1;
                }
                break;
            }
        }
//        ReadEncoder();
//                rt_kprintf("Roll : %d \r\n", (int16_t)Roll);
//                rt_kprintf("Pitch : %d \r\n", (int16_t)Pitch);
//                rt_kprintf("Yaw : %d \r\n", (int16_t)Yaw);
    }
}


/* 发送零偏航角命令 */
void Serial_JY61P_Zero_Yaw(void)
{
    /* 确保设备已初始化 */
    rt_kprintf("JY61P set zero\r\n");
    rt_kprintf("send : start\r\n");
    /* 第一组命令 */
    jy61p_send_byte(0xFF);
    jy61p_send_byte(0xAA);
    jy61p_send_byte(0x69);
    jy61p_send_byte(0x88);
    jy61p_send_byte(0xB5);

    /* 使用RT-Thread系统延时 */
    rt_thread_mdelay(100);

    /* 第二组命令 */
    jy61p_send_byte(0xFF);
    jy61p_send_byte(0xAA);
    jy61p_send_byte(0x01);
    jy61p_send_byte(0x04);
    jy61p_send_byte(0x00);

    rt_thread_mdelay(100);

    /* 第三组命令 */
    jy61p_send_byte(0xFF);
    jy61p_send_byte(0xAA);
    jy61p_send_byte(0x00);
    jy61p_send_byte(0x00);
    jy61p_send_byte(0x00);

    rt_kprintf("send : finish\r\n");
}


/*欧拉角计算*/
void process_euler_angles(void)
{
//    rt_kprintf("start JiSuang\r\n");
    // 数据接收完毕，分配给具体的变量
    RollL = receivedData[0];
    RollH = receivedData[1];
    PitchL = receivedData[2];
    PitchH = receivedData[3];
    YawL = receivedData[4];
    YawH = receivedData[5];
    VL = receivedData[6];
    VH = receivedData[7];
    SUM = receivedData[8];

    // 校验SUM是否正确
    uint8_t calculatedSum = 0x55 + 0x53 + RollH + RollL + PitchH + PitchL + YawH + YawL + VH + VL;
    if (calculatedSum == SUM) {
//        rt_kprintf("Checksum OK\n");
        // 校验成功，可以进行后续处理
        if((float)(((uint16_t)RollH << 8) | RollL)/32768*180>180){
            Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180 - 360;
        }else{
            Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180;
        }
//        rt_kprintf("Roll : %d \r\n", (int16_t)Roll);
        if((float)(((uint16_t)PitchH << 8) | PitchL)/32768*180>180){
            Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180 - 360;
        }else{
            Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180;
        }
//        rt_kprintf("Pitch : %d \r\n", (int16_t)Pitch);
        if((float)(((uint16_t)YawH << 8) | YawL)/32768*180 >180){
            Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180 - 360;
        }else{
            Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180;
        }
//        rt_kprintf("Yaw : %d \r\n", (int16_t)Yaw);
    }else {
        rt_kprintf("Checksum failed. Expected: %d, Got: %d\n", calculatedSum, SUM);
    }
}



/*****************************DMA加信号量***********************************************/

/* 陀螺仪线程入口函数 */
//void Gyroscope_task(void *parameter)
//{
//    rt_tick_t last_wake_time = rt_tick_get();  // 获取系统启动后的当前节拍值
//    while(1)
//    {
//        //此任务以100Hz的频率运行，参数写的tick,即为10ms
//        rt_thread_delay_until(&last_wake_time, 10);
//        JY61P_data_process_thread_entry(); //得到陀螺仪数据
//
//    }
//}

/*DMA中断回调接收函数*/
//rt_err_t uart2_dma_callback(rt_device_t dev, rt_size_t size)
//{
//    /* DMA传输完成时触发信号量 */
////    static int count;
////    rt_kprintf("recevive: count: %d\r\n", count++);
//    rt_sem_release(&rx_sem);
//    return RT_EOK;
//}

/*硬件接口初始化*/
//int uart2_dma_init(void)
//{
//    /* 查找串口设备 */
//    serial = rt_device_find(SAMPLE_UART_NAME);
//    if (!serial) {
//        rt_kprintf("UART device %s not found!\n", SAMPLE_UART_NAME);
//        return RT_ERROR;
//    }
//
//    /* 配置串口参数 */
//    struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;
//    config.baud_rate = BAUD_RATE_9600;
//    config.bufsz = 256;
//    rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config);
//
//    /* 以DMA接收+轮询发送模式打开设备 */
//    if (rt_device_open(serial, RT_DEVICE_FLAG_DMA_RX) != RT_EOK) {
//        rt_kprintf("Failed to open UART!\n");
//        return RT_ERROR;
//    }
//
//    /* 初始化信号量 */
//    rt_sem_init(&rx_sem, "dma_rx", 0, RT_IPC_FLAG_FIFO);
//
//    /* 设置接收回调 */
//    rt_device_set_rx_indicate(serial, uart2_dma_callback);
//
//    rt_kprintf("Gyroscope enable succeed\r\n");
//
//    return RT_EOK;
//}

/*欧拉角计算*/
//void process_euler_angles(void)
//{
////    rt_kprintf("start JiSuang\r\n");
//    // 数据接收完毕，分配给具体的变量
//    RollL = receivedData[0];
//    RollH = receivedData[1];
//    PitchL = receivedData[2];
//    PitchH = receivedData[3];
//    YawL = receivedData[4];
//    YawH = receivedData[5];
//    VL = receivedData[6];
//    VH = receivedData[7];
//    SUM = receivedData[8];
//
//    // 校验SUM是否正确
//    uint8_t calculatedSum = 0x55 + 0x53 + RollH + RollL + PitchH + PitchL + YawH + YawL + VH + VL;
//    if (calculatedSum == SUM) {
////        rt_kprintf("Checksum OK\n");
//        // 校验成功，可以进行后续处理
//        if((float)(((uint16_t)RollH << 8) | RollL)/32768*180>180){
//            Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180 - 360;
//        }else{
//            Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180;
//        }
////        rt_kprintf("Roll : %d \r\n", (int16_t)Roll);
//        if((float)(((uint16_t)PitchH << 8) | PitchL)/32768*180>180){
//            Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180 - 360;
//        }else{
//            Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180;
//        }
////        rt_kprintf("Pitch : %d \r\n", (int16_t)Pitch);
//        if((float)(((uint16_t)YawH << 8) | YawL)/32768*180 >180){
//            Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180 - 360;
//        }else{
//            Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180;
//        }
////        rt_kprintf("Yaw : %d \r\n", (int16_t)Yaw);
//    }else {
//        rt_kprintf("Checksum failed. Expected: %d, Got: %d\n", calculatedSum, SUM);
//    }
//}

/* 阻塞发送单字节 */
//void jy61p_send_byte(uint8_t data)
//{
//    rt_size_t sent = rt_device_write(serial, 0, &data, 1);
//    if (sent != 1) {
//        rt_kprintf("[WARN] Send failed: 0x%02X\n", data);
//    }
//}

///* 发送零偏航角命令 */
//void Serial_JY61P_Zero_Yaw(void)
//{
//    /* 确保设备已初始化 */
//    rt_kprintf("JY61P set zero\r\n");
//    rt_kprintf("send : start\r\n");
//    /* 第一组命令 */
//    jy61p_send_byte(0xFF);
//    jy61p_send_byte(0xAA);
//    jy61p_send_byte(0x69);
//    jy61p_send_byte(0x88);
//    jy61p_send_byte(0xB5);
//
//    /* 使用RT-Thread系统延时 */
//    rt_thread_mdelay(100);
//
//    /* 第二组命令 */
//    jy61p_send_byte(0xFF);
//    jy61p_send_byte(0xAA);
//    jy61p_send_byte(0x01);
//    jy61p_send_byte(0x04);
//    jy61p_send_byte(0x00);
//
//    rt_thread_mdelay(100);
//
//    /* 第三组命令 */
//    jy61p_send_byte(0xFF);
//    jy61p_send_byte(0xAA);
//    jy61p_send_byte(0x00);
//    jy61p_send_byte(0x00);
//    jy61p_send_byte(0x00);
//
//    rt_kprintf("send : finish\r\n");
//}

/*数据处理*/
//void JY61P_data_process_thread_entry(void)
//{
//    uint8_t data_index = 0;
//    uint8_t rx_state = WAIT_HEADER1;
//
//    while (1) {
//        /* 等待DMA接收完成信号 */
//        rt_sem_take(&rx_sem, RT_WAITING_FOREVER);
////        rt_kprintf("take\r\n");
//
//        /* 读取DMA缓冲区数据 */
//        rt_size_t len = rt_device_read(serial, 0, rx_buffer, RX_BUFFER_SIZE);
//
//        /* 协议解析状态机 */
//        for (int i = 0; i < len; i++) {
//            uint8_t data = rx_buffer[i];
//            switch (rx_state) {
//            case WAIT_HEADER1:
//                if (data == 0x55)
//                {
//                    rx_state = WAIT_HEADER2;
////                    rt_kprintf("1\r\n");
//                }
//                break;
//            case WAIT_HEADER2:
//                rx_state = (data == 0x53) ? RECEIVE_DATA : WAIT_HEADER1;
//                data_index = 0;
//                break;
//            case RECEIVE_DATA:
//                receivedData[data_index++] = data;
//                if (data_index >= 9) {
//                    process_euler_angles(); // 欧拉角计算
//                    rx_state = WAIT_HEADER1;
//                }
//                break;
//            }
//        }
////        rt_kprintf("Roll : %d \r\n", (int16_t)Roll);
////        rt_kprintf("Pitch : %d \r\n", (int16_t)Pitch);
////        rt_kprintf("Yaw : %d \r\n", (int16_t)Yaw);
//    }
//}

//MSH_CMD_EXPORT(JY61P_data_process_thread_entry,JY61P_data_process_thread_entry);
